#include "./handler_task.h"
#include "../pub_service/ota_service.h"
#include "../main_driver/ctrl.h"
#include "../main_driver/motors.h"
#include "../main_driver/cmd_uart.h"

namespace main_task {

using namespace main_driver;
using namespace pub_service;
using namespace conn;

HandlerTask handlerTask;

void HandlerTask::init_start() {
    m_mq.init();
    start();
}

static void on_parts_ctrl(const Frame &frame) {
    uint8_t parts, value;
    Error err = frame.parse(parts, value);
    if (err) {
        goto finish;
    }

    if ((parts > 3) || (value > 1)) {
        err = ec::pub::INVALID;
        goto finish;
    }

    if (value == 0) {
        ctrl.open_led( static_cast<Ctrl::Parts>(parts) ); 
    } else {
        ctrl.close_led( static_cast<Ctrl::Parts>(parts) );
    }

finish:
    cmdUart.fast_res(frame, err);   
}

static void on_pick_door_ctrl(const Frame &frame) {
    uint8_t type, val;
    uint16_t v;
    Error err = frame.parse(type, val);
    if (err) {
        goto finish;
    }

    v = (type << 8) + val;
    switch (v) {
        case 0x0000: err = motors.pick1_open(); break;
        case 0x0001: err = motors.pick1_close(); break;
        case 0x0100: err = motors.pick2_open(); break;
        case 0x0101: err = motors.pick2_close(); break;
        case 0x0200: err = motors.straw_open(); break;
        case 0x0201: err = motors.straw_close(); break;
        default: err = ec::pub::INVALID; break;
    }   
finish:
    cmdUart.fast_res(frame, err);
}

static void on_drop_cup(const Frame &frame) {
    uint8_t type;
    Error err = frame.parse(type);
    if (err) {
        goto finish;
    }

    switch (type) {
        case 0: err = ctrl.drop1(); break;
        case 1: err = ctrl.drop2(); break;
        default: err = ec::pub::INVALID; break;
    }   
finish:
    cmdUart.fast_res(frame, err);
}

static void on_film(const Frame &frame) {
    Error err;

    err = motors.film();
finish:
    cmdUart.fast_res(frame, err);
}

static void on_lock_ctrl(const Frame &frame) {
    ctrl.lock_open();
    cmdUart.fast_res(frame, ec::pub::OK);
}

static void on_robot_ctrl(const Frame &frame) {
    ctrl.robot_reset();
    cmdUart.fast_res(frame, ec::pub::OK);
}

void HandlerTask::run() {

    Frame frame;
    for (;;) {
        m_mq.poll(frame);
        task_info.seq = frame.seq_cmd();
        task_info.cmd = frame.cmd();

        switch (frame.cmd()) {
            case proto::main::LOCK_CTRL: on_lock_ctrl(frame); break;
            case proto::main::ROBOT_CTRL: on_robot_ctrl(frame); break;
            case proto::main::FILM_CTRL: on_film(frame); break;
            case proto::main::DROP_CUP_CTRL: on_drop_cup(frame); break;
            case proto::main::PICK_DOOR_CTRL: on_pick_door_ctrl(frame); break;
            case proto::main::PARTS_CTRL: on_parts_ctrl(frame); break;
            case proto::pub::OTA_START: pub_service::ota_start(frame); break;
            case proto::pub::OTA_TRANSLATE: pub_service::ota_translate(frame); break;
            case proto::pub::OTA_COMPLETE: pub_service::ota_complete(frame); break;
        }
    
        task_info.reset();
    }
}

}
